#include "_includes.h"

// 系统变量
vex::brain Brain;
vex::competition Competition;
////////////////////////////////////////////////////////////////////////
unsigned int sec_n = 0;
bool ctrl_screen_display_err_info = false;  // 其他函数临时借用遥控屏幕显示
Clock clock_ctrl_disp;                      // 遥控显示运行时间

void test();

void ctrl_clock_disp_reset() {
  clock_ctrl_disp.reset();
  sec_n = 0;
}

int main() {
  // 程序运行到这里时间会有随机差异,使用系统时间初始化随机数种子
  srand(Brain.Timer.systemHighResolution());
  ctrl.Screen.clearScreen();
  pre_auton();
  if (1) {
    Competition.autonomous(_autonomous);
    Competition.drivercontrol(_usercontrol);
  }

  int k = 0;
  wait(100);
  controller_print_team_number_and_autoinfo();
  ////////////////////////////////
  while (1) {
    ////////////////////////////////
    if (!ctrl_screen_display_err_info) {
      // 遥控器第一行显示
      if (clock_ctrl_disp.get_msec_timing() > sec_n * 1000) {
        sec_n++;
        ctrl.Screen.setCursor(1, 1);
        if (competition_on)
          ctrl.Screen.print(
              "B: %2d%%  T: %02d:%02d  ON ", (int)Brain.Battery.capacity(),
              clock_ctrl_disp.get_min_disp(), clock_ctrl_disp.get_sec_disp());
        else
          ctrl.Screen.print(
              "B: %2d%%  T: %02d:%02d  OFF", (int)Brain.Battery.capacity(),
              clock_ctrl_disp.get_min_disp(), clock_ctrl_disp.get_sec_disp());
      }

      // 遥控器第二行显示
      if (k % 3 == 0 && !autonomous_running) {
        ctrl.Screen.setCursor(2, 1);
        if (imu.installed()) {
          ctrl.Screen.print("g: %-6.2fd: %d mm      ", get_gyro(),
                            (int)chassis.get_dist_mm(-1));
        } else
          ctrl.Screen.print("g: error d: %d mm      ",
                            (int)chassis.get_dist_mm(-1));
      }

      // 视觉传感器闪烁
      if (vis.installed()) {  // 视觉传感器颜色显示
        if (k % 2 == 0) {
          if (k % 3 == 0)
            vis_get_lowest_cdx(vis, vis_RED);
          else if (k % 3 == 1)
            vis_get_lowest_cdx(vis, vis_YELLOW);
          else if (k % 3 == 2)
            vis_get_lowest_cdx(vis, vis_BLUE);
        }
      }
      //////////////////////////
      if (k % 3 == 0) controller_print_team_number_and_autoinfo();
    }
    ++k;
    wait(100);
  }
}
